#ifndef CORE_H
#define CORE_H

#include <QFile>
#include <QObject>

class vector3D;
class coord3D;
class Robot;
class Joint;

class Core : public QObject
{
	Q_OBJECT
	
	public:
	
		Core(QObject *mainwin, Robot *Robot);
		~Core(); // Destructor
		
		void init();
		
		void setRobotPointer(Robot *Robot);
		void loadBlankRobot();
		void loadScaraRobot();
                bool loadRobotFromFile(QString filename);

               

              
		
	public slots:
		void adjustJoint(int joint, float value);
		void exportData(int flags);
		
	private slots:
		
	signals:
		void robotUpdated();
		void robotLoaded();
		
		
	private:
		QObject *parent;
		Robot *robot;
                void readstr(FILE* f, char* string);

                QFile *exportFile;

                float round(float number);
		int roundPrecision;
		
		//update joints in robot with new centers and orientations
                // (after some modification)
                void recalculateJoints();
                coord3D findCenter(int jointNumber, double &output);
                vector3D findKAxisOrientation(double &output);
		
		//Calculation helper functions
                void Copy_Matrix(double *A, double *B, int nrows, int ncols);
                void Transpose_Matrix(double *At, double *A, int nrows, int ncols);
                void Multiply_Matrices(double *C, double *A, int nrows, int ncols, double *B, int mcols);
                void Jacobi_Cyclic_Method(double eigenvalues[], double *eigenvectors, double *A, int n);
                double pythag(double a, double b);
                void svdcmp(double **a, int m, int n, double w[], double **v);
                void Cross_Product(double &M1, double &M2, double &Cross);
                double Get_Condition_Number(double &A, double &A_inv, int height, int width);
                double Get_Norm(double &A, int height, int width);

		void makeTransformMatrixFromDHParams(int jointNumber); 
                void modifyTransformMatrix(int jointNumber, double Rotation, double Extension);

                int inverseKinematics(double desired_gripper_pos[3], double desired_gripper_frame_i[3], double desired_gripper_frame_j[3], double desired_gripper_frame_k[3]);

                void makeJacobian();
		
                // This is the number of the last link in the Robot (Range = 0...6)
                int Last_Link;

                // This array holds the type of each joint. (0 = revolute, 1 = prismatic, 2 = gripper, 3 = no joint)
                char type[7];

                // Every transformation matrix. (smaller robots will not use all the matricies)
                double T1[4][4];
                double T2[4][4];
                double T3[4][4];
                double T4[4][4];
                double T5[4][4];
                double T6[4][4];
                double T7[4][4];

                // The generated robot Jacobian at its current position (Smaller robots will not fill the matrix)
                double Jacobian[6][7];

                // This variable stores all of the current joint parameters of the robot
                double Q[7];

                // Each row of this matrix represents a different joint.
                // The first 3 columns are the center of the joint, and the last 9 columns are the i,j,k axis
                double absolute_Params[7][12];

                // This matrix stores the DH parameters loaded from the file
                double DHList[7][4];
                // This matrix stores the actuation lower and upper limit
                double Limits[7][2];
                
                // Animation variables
                int animationJoint;
                int animationPriority;
                double animationStart;
                double animationStop;

};

#endif
